/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       motor.c/h
  * @brief      电机相关部分定义
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Apr-1-2024      Penguin         1. done
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  */
#ifndef MOTOR_H
#define MOTOR_H

#include "pid.h"
#include "robot_typedef.h"
#include "stdbool.h"
#include "struct_typedef.h"

#define RPM_TO_OMEGA 0.1047197551f    // (1/60*2*pi) (rpm)->(rad/s)
#define DEGREE_TO_RAD 0.0174532925f   // (pi/180) (degree)->(rad)
#define RAD_TO_DEGREE 57.2957795131f  // (180/pi) (rad)->(degree)

#define MOTOR_STABLE_RUNNING_TIME 10  // (ms)电机稳定运行时间

/*-------------------- DJI Motor --------------------*/

// 电机编码值规整 0—8191
#define ecd_format(ecd)         \
    {                           \
        if ((ecd) > ECD_RANGE)  \
            (ecd) -= ECD_RANGE; \
        else if ((ecd) < 0)     \
            (ecd) += ECD_RANGE; \
    }

#define DJI_GM6020_ECD_TO_RAD 0.000766990394f  // (2*pi/8192) 电机编码器值转换为弧度
#define DJI_GM3508_RPM_TO_OMEGA 0.0055115661f  // (1/60*2*pi/19) m3508(减速比19:1) (rpm)->(rad/s)
#define DJI_GM2006_RPM_TO_OMEGA 0.0029088821f  // (1/60*2*pi/36) m2006(减速比36:1) (rpm)->(rad/s)

#define DJI_GM2006_MAX_CURRENT 10000
#define DJI_GM3508_MAX_CURRENT 16384
#define DJI_GM6020_MAX_VOLTAGE 30000

// 电机模式
#define DJI_3508_MODE_CURRENT_1 ((uint16_t)0x200)  // 3508电流控制模式
#define DJI_2006_MODE_CURRENT_1 ((uint16_t)0x200)  // 2006电流控制模式

#define DJI_3508_MODE_CURRENT_2 ((uint16_t)0x1FF)  // 3508电流控制模式
#define DJI_2006_MODE_CURRENT_2 ((uint16_t)0x1FF)  // 2006电流控制模式

#define DJI_6020_MODE_VOLTAGE_1 ((uint16_t)0x1FF)  // 6020电压控制模式
#define DJI_6020_MODE_VOLTAGE_2 ((uint16_t)0x2FF)  // 6020电压控制模式

// #define DJI_6020_MODE_CURRENT_1 0x2FE  // 6020电流控制模式

typedef struct _DjiMotorMeasure
{
    uint16_t ecd;
    int16_t speed_rpm;
    int16_t given_current;
    uint8_t temperate;
    int16_t last_ecd;

    uint32_t last_fdb_time;  //上次反馈时间
} DjiMotorMeasure_t;

/*-------------------- Motor struct --------------------*/

/**
 * @brief  通用电机结构体
 * @note   包括电机的信息、状态量和控制量
 * @note   电机信息部分的参数不影响电机的反馈数据，由用户自行使用与处理
 */
typedef struct __Motor
{
    /*电机信息*/
    uint8_t id;             // 电机ID
    MotorType_e type;       // 电机类型
    uint8_t can;            // 电机所用CAN口
    float reduction_ratio;  // 电机减速比，例如2006为36:1，则reduction_ratio=36
    int8_t direction;       // 电机旋转方向（1或-1）
    uint16_t mode;          // 电机模式
    bool offline;           // 电机是否离线

    /*状态量*/
    struct __fdb
    {
        float acc;      // (rad/s^2)电机加速度
        float vel;      // (rad/s)电机输出轴转速
        float tor;      // (N*m)电机输出力矩
        float pos;      // (rad)电机输出轴位置
        float temp;     // (℃)电机温度
        float curr;     // (A)电机电流
        int16_t round;  // (r)电机旋转圈数(用于计算输出轴位置)
        uint16_t ecd;   // 电机编码器值
        uint8_t state;  // 电机状态
    } fdb;

    /*设定值*/
    struct __set
    {
        float curr;   // (A)电机电流
        float volt;   // (V)电机电压
        float tor;    // (N*m)电机力矩
        float vel;    // (rad/s)电机输出轴转速
        float pos;    // (rad)电机位置
        float value;  // 可发送的直接控制量，无单位
    } set;

} Motor_s;

/*-------------------- Motor function --------------------*/

extern void MotorInit(
    Motor_s * p_motor, uint8_t id, uint8_t can, MotorType_e motor_type, int8_t direction,
    float reduction_ratio, uint16_t mode);

#endif  // MOTOR_H
